#include "TestMatchers.h"
#include <iostream>
#include <boost/random/normal_distribution.hpp>
#include <boost/random/mersenne_twister.hpp>
#include <boost/random/variate_generator.hpp>

boost::mt19937 igen;
boost::variate_generator<boost::mt19937, boost::normal_distribution<> >angle_random					(igen,boost::normal_distribution<>(0,0.4));
boost::variate_generator<boost::mt19937, boost::normal_distribution<> >translation_random			(igen,boost::normal_distribution<>(0,0.05));
boost::variate_generator<boost::mt19937, boost::normal_distribution<> >particle_position_random		(igen,boost::normal_distribution<>(0,0.003));//0.011 works
boost::variate_generator<boost::mt19937, boost::normal_distribution<> >feature_random				(igen,boost::normal_distribution<>(0,0.04));//0.04->0.045 is reasonable

boost::variate_generator<boost::mt19937, boost::normal_distribution<> >selection_random				(igen,boost::normal_distribution<>(0,100));
const float chance_of_shared_keypoint = 0.25f;
const double debugg_TestMatchers = false;

void MatcherStatistics::print()
{
	printf("========================================\n");
	printf("Name: %s\n",name.c_str());
	printf("avg time consumtion %f\n",time_consumption / (double)executions);
	printf("correct matches     %i\n",correct_matches);
	printf("possible matches    %i\n",possible_matches);
	printf("incorrect matches   %i\n",incorrect_matches);
	printf("%f percent of correct matches found\n%f percent of matches found were incorrect\n",(100.0f*(double)correct_matches)/(double)possible_matches,100.0f*((double)incorrect_matches)/(double)(correct_matches+incorrect_matches));
}

void MatcherStatistics::update(Transformation * ideal, Transformation * found, double time)
{
	printf("update statistics\n");
	int found_nr_matches = found->nr_matches;
	int ideal_nr_matches = ideal->nr_matches;
	KeyPoint ** ideal_src_keypoint = ideal->matches_src;
	KeyPoint ** ideal_dst_keypoint = ideal->matches_dst;
	KeyPoint ** found_src_keypoint = found->matches_src;
	KeyPoint ** found_dst_keypoint = found->matches_dst;
	time_consumption+=time;
	executions++;
	possible_matches+=ideal_nr_matches;
	for(int i = 0; i < ideal_nr_matches ;i++)
	{
		KeyPoint * src_keypoint = ideal_src_keypoint[i];
		KeyPoint * dst_keypoint = ideal_dst_keypoint[i];
		bool found = false;
		for(int j = 0; j < found_nr_matches;j++)
		{
		
			if(src_keypoint == found_src_keypoint[j] && dst_keypoint == found_dst_keypoint[j])
			{
				correct_matches++;
				break;
			}
		
		}
	}
	
	for(int i = 0; i < found_nr_matches ;i++)
	{
		KeyPoint * src_keypoint = found_src_keypoint[i];
		KeyPoint * dst_keypoint = found_dst_keypoint[i];
		bool found = false;
		for(int j = 0; j < ideal_nr_matches;j++)
		{
		
			if(src_keypoint == ideal_src_keypoint[j] && dst_keypoint == ideal_dst_keypoint[j])
			{
				found = true;
				break;
			}
		
		}
		if(!found){incorrect_matches++;}
	}
	
	
	print();
}

TestMatchers::TestMatchers(RGBDCamera * camera)
{
	this->camera = camera;
	nr_matchers = 3;
	matchers = new FrameMatcher*[nr_matchers];
	matchers[0] = new SurfRansacMatcherI();
	matchers[1] = new SurfIcpMatcherI();
	matchers[2] = new SurfIcpMatcherII();
	statistics =  new MatcherStatistics *[nr_matchers];
	for(int i  = 0; i< nr_matchers ; i++)
	{
		statistics[i] = new MatcherStatistics();
		statistics[i]->name = matchers[i]->name;
		statistics[i]->correct_matches = 0;
		statistics[i]->incorrect_matches = 0;
		statistics[i]->possible_matches = 0;
		statistics[i]->executions = 0;
	}
	//matchers[2] = new BasicIcpMatcher();
	
	if(debugg_TestMatchers){printf("new TestMatchers\n");}
}

void TestMatchers::test()
{
	pcl::visualization::CloudViewer viewer("Cloud Viewer");
	//for(int i = 0; i < 10000; i++){printf("randn number: %f\n",gen_mean0_sigma1());}
	double * time_consumption = new double[nr_matchers];
	for(int i = 0; i < nr_matchers; i++)
	{
		time_consumption[0] = 0;
	}
	if(debugg_TestMatchers)
	{
		printf("___________________________________________________________________________________________________________________\n");
		printf("====================================================Start test=====================================================\n");
	}
	for(int i = 0; i < camera->nr_frames; i++)
	{
		if(debugg_TestMatchers){printf("========================:%i:========================\n",i);}
		RGBDFrame * frame_src 		= camera->getNextFrame();
		RGBDFrame * frame_dst 		= camera->getCurrentFrame();
		
		FrameNode * src_node 		= new FrameNode();
		src_node->matrix 			= Eigen::Matrix4f::Identity();
		src_node->frame 			= frame_src;
		src_node->next 				= 0;
		src_node->prev 				= 0;
		src_node->transformation 	= 0;
		src_node->locked 			= false;
		src_node->id 				= frame_src->id_number;
		
		FrameNode * dst_node 		= new FrameNode();
		dst_node->matrix 			= Eigen::Matrix4f::Identity();
		dst_node->frame 			= frame_dst;
		dst_node->next 				= 0;
		dst_node->prev 				= 0;
		dst_node->transformation 	= 0;
		dst_node->locked 			= false;
		dst_node->id 				= frame_dst->id_number;
		
		Transformation * trans 		= scramble(src_node, dst_node);
		


		
		for(int j = 0; j < nr_matchers; j++)
		{
			//printf("_________________________\n");
			double t1 = get_time_now();
			Transformation * transformation = matchers[j]->getTransformation(src_node, dst_node);
			double t2 = get_time_now();
			//time_consumption[j] += t2-t1;
			//printf("%f, ",time_consumption[j]/(double)i);
			statistics[j]->update(trans, transformation, t2-t1);
			if(debugg_TestMatchers){
				while(transformation != 0)
				{
					transformation->print();
					//transformation->display();
					transformation->displayDifference(trans);
				
					pcl::PointCloud<pcl::PointXYZRGB>::Ptr src_cloud = frame_src->getCloud();
					pcl::PointCloud<pcl::PointXYZRGB>::Ptr full_cloud 	(new pcl::PointCloud<pcl::PointXYZRGB>);
					pcl::PointCloud<pcl::PointXYZRGB>::Ptr result2 		(new pcl::PointCloud<pcl::PointXYZRGB>);
					pcl::transformPointCloud (*(frame_src->getCloud()), *result2, trans->transformationMatrix * transformation->transformationMatrix);

					for(int i = 0; i < result2->width; i++)
					{
						result2->points[i].r = 0;
						result2->points[i].g = 255;
						result2->points[i].b = 0;
					}

					*full_cloud += *result2;
			
					pcl::transformPointCloud (*(frame_src->getCloud()), *result2, trans->transformationMatrix);
					for(int i = 0; i < result2->width; i++)
					{
						result2->points[i].r = 0;
						result2->points[i].g = 0;
						result2->points[i].b = 255;
						result2->points[i].x += particle_position_random();
						result2->points[i].y += particle_position_random();
						result2->points[i].z += particle_position_random();
					}

					*full_cloud += *result2;
			
					pcl::PointCloud<pcl::PointXYZRGB>::Ptr tmp = frame_dst->getCloud();
					for(int i = 0; i < tmp->width; i++)
					{
						tmp->points[i].r = 255;
						tmp->points[i].g = 0;
						tmp->points[i].b = 0;
					}
					*full_cloud += *tmp;
					viewer.showCloud(full_cloud);
					printf("weight: %f\n",transformation->weight);
					cvWaitKey(0);
					transformation = transformation->next;
					//sleep(1);
				}
			}
			//sleep(5);
		}
		printf("\n");
		
	}
}
//using namespace Eigen;



Transformation * TestMatchers::scramble(FrameNode * src_node, FrameNode * dst_node)
{
	RGBDFrame * src = src_node->frame;
	RGBDFrame * dst = dst_node->frame;
	int startpoint_a = rand()%1000;
	int startpoint_b = rand()%1000;
	int startpoint_c = rand()%1000;
	int startpoint_d = rand()%1000;
	
	for(int i = 0; i < startpoint_a; i++){translation_random();}
	for(int i = 0; i < startpoint_b; i++){angle_random();}
	for(int i = 0; i < startpoint_c; i++){particle_position_random();}
	for(int i = 0; i < startpoint_d; i++){selection_random();}

	IplImage* img_col = cvLoadImage( src->rgb_path.c_str(), 1);
	int src_mid_w 	= rand() % src->width;
	int src_mid_h 	= rand() % src->height;
	float aspect_ratio_src = ((float)src->width)/((float)src->height);
	int src_w 		= src->width/2 + aspect_ratio_src*fabs(selection_random());
	int src_h 		= src->height/2 + fabs(selection_random());
	
	int dst_mid_w = rand() % dst->width;
	int dst_mid_h = rand() % dst->height;
	float aspect_ratio_dst = ((float)dst->width)/((float)dst->height);
	int dst_w 		= dst->width/3 + aspect_ratio_src*fabs(selection_random());
	int dst_h 		= dst->height/3 + fabs(selection_random());
	
	cvRectangle(img_col,cvPoint(src_mid_w-src_w, src_mid_h-src_h),cvPoint(src_mid_w+src_w, src_mid_h+src_h),cvScalar(255, 255, 0, 0),5, 8, 0);
	cvRectangle(img_col,cvPoint(src_mid_w-10, src_mid_h-10),cvPoint(src_mid_w+10, src_mid_h+10),cvScalar(255, 255, 0, 0),5, 8, 0);
	
	cvRectangle(img_col,cvPoint(dst_mid_w-dst_w, dst_mid_h-dst_h),cvPoint(dst_mid_w+dst_w, dst_mid_h+dst_h),cvScalar(0, 255, 255, 0),5, 8, 0);
	cvRectangle(img_col,cvPoint(dst_mid_w-15, dst_mid_h-10),cvPoint(dst_mid_w+15, dst_mid_h+10),cvScalar(0, 255, 255, 0),5, 8, 0);

	
	float angle1 = angle_random();
	float angle2 = angle_random();
	float angle3 = angle_random();
	if(debugg_TestMatchers){printf("angles: %f %f %f\n",angle1,angle2,angle3);}
	float trans1 = translation_random();
	float trans2 = translation_random();
	float trans3 = translation_random();
	if(debugg_TestMatchers){printf("trans: %f %f %f\n",trans1,trans2,trans3);}
	Eigen::Matrix3f m;
	m = Eigen::AngleAxisf(angle1, Eigen::Vector3f::UnitZ()) * Eigen::AngleAxisf(angle2, Eigen::Vector3f::UnitY()) * Eigen::AngleAxisf(angle3, Eigen::Vector3f::UnitZ());
	
	Eigen::Matrix4f transformation_mat = Eigen::Matrix4f::Identity();
	transformation_mat(0,3) = trans1;
	transformation_mat(1,3) = trans2;
	transformation_mat(2,3) = trans3;
	
	if(debugg_TestMatchers){printf("_________________________scramble_________________________\n");}
	for(int i = 0; i < 3; i++)
	{
		for(int j = 0; j < 3; j++)
		{
			transformation_mat(i,j) = m(i,j);
		}
	}
	if(debugg_TestMatchers){
		for(int i = 0; i < 4; i++)
		{
			for(int j = 0; j < 4; j++)
			{
				if(transformation_mat(i,j) >= 0){printf(" ");}
				printf("%5.5f   ",transformation_mat(i,j));
			}
			printf("\n");
		}
		printf("__________________________________________________________\n");
	}
	
	float mat00 = transformation_mat(0,0);
	float mat01 = transformation_mat(0,1);
	float mat02 = transformation_mat(0,2);
	float mat03 = transformation_mat(0,3);
	float mat10 = transformation_mat(1,0);
	float mat11 = transformation_mat(1,1);
	float mat12 = transformation_mat(1,2);
	float mat13 = transformation_mat(1,3);
	float mat20 = transformation_mat(2,0);
	float mat21 = transformation_mat(2,1);
	float mat22 = transformation_mat(2,2);
	float mat23 = transformation_mat(2,3);
	float mat30 = transformation_mat(3,0);
	float mat31 = transformation_mat(3,1);
	float mat32 = transformation_mat(3,2);
	float mat33 = transformation_mat(3,3);
	
	for(int i = 0; i < src->keypoints->nr_valid_points; i++){
		KeyPoint * point 	= src->keypoints->valid_key_points[i];

		float x 			= point->px + particle_position_random();
		float y 			= point->py + particle_position_random();
		float z 			= point->pz + particle_position_random();
				
		float dx 			= x*mat00+y*mat01+z*mat02+mat03;
		float dy 			= x*mat10+y*mat11+z*mat12+mat13;
		float dz 			= x*mat20+y*mat21+z*mat22+mat23;
		
		point->px			= dx;
		point->py			= dy;
		point->pz			= dz;
		point->position(0) 	= dx;
		point->position(1) 	= dy;
		point->position(2) 	= dz;
		
		FeatureDescriptor * descriptorA = point->surf_descriptor;
		for(int j = 0; j < descriptorA->descriptor_length;j++)
		{
			descriptorA->descriptor[j] += feature_random();
		}
	}
	
	for(int i = 0; i < dst->keypoints->nr_valid_points; i++){
	}
	
	for(int i = 0; i < src->keypoints->nr_valid_points; i++){
		FeatureDescriptor * descriptorA = src->keypoints->valid_key_points[i]->surf_descriptor;
		FeatureDescriptor * descriptorB = dst->keypoints->valid_key_points[i]->surf_descriptor;
		//printf("corresponding: %i:%f\n",i,descriptorA->distance(descriptorB));
	}
	
	
	KeyPointSet * keypoints_src 			= new KeyPointSet();
	keypoints_src->stabilety_threshold 		= src->keypoints->stabilety_threshold;
	keypoints_src->valid_key_points 		= new KeyPoint*[src->keypoints->nr_valid_points];
	keypoints_src->invalid_key_points 		= new KeyPoint*[src->keypoints->nr_invalid_points];
	keypoints_src->nr_valid_points 			= 0;
	keypoints_src->nr_invalid_points 		= 0;

	KeyPointSet * keypoints_dst 			= new KeyPointSet();
	keypoints_dst->stabilety_threshold 		= dst->keypoints->stabilety_threshold;
	keypoints_dst->valid_key_points 		= new KeyPoint*[dst->keypoints->nr_valid_points];
	keypoints_dst->invalid_key_points 		= new KeyPoint*[dst->keypoints->nr_invalid_points];
	keypoints_dst->nr_valid_points 			= 0;
	keypoints_dst->nr_invalid_points 		= 0;
	
	KeyPoint ** correspondance_list_src = new KeyPoint*[src->keypoints->nr_valid_points];
	KeyPoint ** correspondance_list_dst = new KeyPoint*[src->keypoints->nr_valid_points];
	int nr_correspondance = 0;
		
	for(int i = 0; i < src->keypoints->nr_valid_points; i++)
	{
		KeyPoint * kp_src= src->keypoints->valid_key_points[i];
		KeyPoint * kp_dst= dst->keypoints->valid_key_points[i];
		bool shared = ((float)(rand()%10000))/10000.0f < chance_of_shared_keypoint;
		if(shared){
			//cvCircle(img_col,cvPoint(kp_src->frame_x, kp_src->frame_y), 6,cvScalar(0, 255, 0, 0),2, 8, 0);
			keypoints_src->valid_key_points[keypoints_src->nr_valid_points] = kp_src;
			keypoints_src->nr_valid_points++;
			
			//cvCircle(img_col,cvPoint(kp_dst->frame_x, kp_dst->frame_y), 3,cvScalar(0, 0, 255, 0),2, 8, 0);
			keypoints_dst->valid_key_points[keypoints_dst->nr_valid_points] = kp_dst;
			keypoints_dst->nr_valid_points++;
			
			cvCircle(img_col,cvPoint(kp_dst->frame_x, kp_dst->frame_y), 2,cvScalar(255, 0, 255, 0),2, 8, 0);
			
			correspondance_list_src[nr_correspondance] = kp_src;
			correspondance_list_dst[nr_correspondance] = kp_dst;
			nr_correspondance++; 
		}else if(rand() % 2 == 1){
			cvCircle(img_col,cvPoint(kp_dst->frame_x, kp_dst->frame_y), 2,cvScalar(0, 0, 255, 0),2, 8, 0);
			keypoints_dst->valid_key_points[keypoints_dst->nr_valid_points] = kp_dst;
			keypoints_dst->nr_valid_points++;
		}else{
			cvCircle(img_col,cvPoint(kp_src->frame_x, kp_src->frame_y), 2,cvScalar(0, 255, 0, 0),2, 8, 0);
			keypoints_src->valid_key_points[keypoints_src->nr_valid_points] = kp_src;
			keypoints_src->nr_valid_points++;
		}
	}
	
	if(debugg_TestMatchers){printf("nr_correspondance: %i\n",nr_correspondance);}
	cvNamedWindow("img_col", CV_WINDOW_AUTOSIZE );
	cvShowImage("img_col", img_col);
	cvReleaseImage( &img_col);
	//cvWaitKey(0);
	//cvWaitKey(30);
	src->keypoints = keypoints_src;	
	dst->keypoints = keypoints_dst;
	
	Transformation * trans 		= new Transformation();
	trans->src 					= src_node;
	trans->dst 					= dst_node;
	trans->transformationMatrix = transformation_mat;
	trans->matches_src			= correspondance_list_src;
	trans->matches_dst			= correspondance_list_dst;
	trans->nr_matches			= nr_correspondance;
	return trans;
}

TestMatchers::~TestMatchers()
{
	printf("nr_frames: %i\n",camera->nr_frames);
	printf("delete TestMatchers\n");
}
